#include "apl_pid.h"
#include "bsp_can.h"
#include "bsp_dbus.h"
#include "tim.h"
#include <math.h>

PID_K_t k_speed_chassis = {15, 1.15, 8}, k_speed_gimbal = {150, 2.15, 8}, k_angle_gimbal = {0.05, 0, 0.3};

short output[4] = {0};

void  PID_init_speed(PID *pid, float MAX_OUTPUT, PID_K_t *K) {
	pid->err = 0;
  pid->integral = 0;
  pid->maxIntegral = 500000; 
  pid->maxOutput = MAX_OUTPUT;
  pid->lastErr = 0;
	pid->output = 0; 
  pid->kp = K->KP;
	pid->ki = K->KI;
	pid->kd = K->KD;
}

void PID_init_angle(PID *pid, float MAX_OUTPUT, PID_K_t *K) {
	pid->err = 0;
	pid->integral = 0;
	pid->maxIntegral = 100;
	pid->maxOutput = MAX_OUTPUT;
	pid->lastErr = 0;
	pid->output = 0;
	pid->kp = K->KP;
	pid->ki = K->KI;
	pid->kd = K->KD;
}

float PID_speed_output(PID* pid, float target, float feedback) {
  
  pid->err = target - feedback;
	//if(pid->err < 5 && pid->err > -5) pid->err = 0;
  
	pid->integral += pid->err;
	if(pid->ki * pid->integral < -pid->maxIntegral) pid->integral = -pid->maxIntegral / pid->ki;
  else if(pid->ki * pid->integral > pid->maxIntegral) pid->integral = pid->maxIntegral / pid->ki;

  if(fabs(target) <= 2) pid->integral /= 1.2;

  pid->output = (pid->kp * pid->err) + (pid->ki * pid->integral) + (pid->kd * (pid->err - pid->lastErr));
	pid->lastErr = pid->err;
    
	if(pid->output > pid->maxOutput || pid->output < -pid->maxOutput) pid->output = pid->output < 0 ? -pid->maxOutput : pid->maxOutput;
 
  //if(target == 0) pid->output = 0;
 
	return pid->output;
}

float PID_angle_output(PID * pid, float target, float feedback) {
	pid->err = fabs(target - feedback) < 8191 - fabs(target - feedback) ? target - feedback : - fabs(target - feedback) / (target - feedback) *(8191 - fabs(target - feedback));
	if(pid->err < 3 && pid->err > -3) pid->err = 0;
	
	pid->integral += pid->err;
	/*add limit integral*/
	if(pid->integral > pid->maxIntegral) pid->integral = pid->maxIntegral;
	if(pid->integral < -pid->maxIntegral) pid->integral = -pid->maxIntegral;
	/*----*/
	
	pid->output = (pid->kp * pid->err) + (pid->ki * pid->integral) + (pid->kd * (pid->err - pid->lastErr));
	pid->lastErr = pid->err;
	
	if(pid->output > pid->maxOutput) pid->output = pid->maxOutput;
	else if(pid->output < -pid->maxOutput) pid->output = -pid->maxOutput;
	
	return pid->output;
}
